
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srv_hal_mgr.c
  * @author     baiyang
  * @date       2021-12-28
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "srv_hal_mgr.h"

#if HAL_WITH_IO_MCU
#include <iomcu/gp_iomcu.h>
#endif

#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static srv_hal_mgr srv_mgr;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void srv_hal_ctor()
{
#if HAL_WITH_IO_MCU
    iomcu_ctor();
    if (brd_io_enabled()) {
        iomcu_init();
        // with IOMCU the local (FMU) channels start at 8
        srv_mgr.chan_offset = 8;
    }
#endif

    srv_mgr.iomcu_mode = MODE_PWM_NORMAL;

    srv_mgr._initialised = true;
}

/**
  * @brief       
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
enum safety_state srv_hal_safety_switch_state(void)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        srv_mgr._safety_state = iomcu_get_safety_switch_state();
    }
#endif

    return srv_mgr._safety_state;
}

/*
  force the safety switch on, disabling PWM output from the IO board
*/
bool srv_hal_force_safety_on(void)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        return iomcu_force_safety_on();
    }
    return false;
#else
    srv_mgr._safety_state = SAFETY_DISARMED;
    return true;
#endif
}

/*
  force the safety switch off, enabling PWM output from the IO board
*/
void srv_hal_force_safety_off(void)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        iomcu_force_safety_off();
    }
#else
    srv_mgr._safety_state = SAFETY_ARMED;
#endif
}

/*
  update safety state
 */
void srv_hal_safety_update(void)
{
    uint32_t now = time_millis();
    if (now - srv_mgr.safety_update_ms < 100) {
        // update safety at 10Hz
        return;
    }
    srv_mgr.safety_update_ms = now;

    // remember mask of channels to allow with safety on
    srv_mgr.safety_mask = brd_get_safety_mask();

#ifdef HAL_GPIO_PIN_SAFETY_IN
    // handle safety button
    //bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_IN);
    bool safety_pressed = false;
    if (safety_pressed) {
        if (srv_mgr.safety_press_count < 255) {
            srv_mgr.safety_press_count++;
        }
        if (brd_safety_button_handle_pressed(srv_mgr.safety_press_count)) {
            if (srv_mgr._safety_state ==SAFETY_ARMED) {
                srv_mgr._safety_state = SAFETY_DISARMED;
            } else {
                srv_mgr._safety_state = SAFETY_ARMED;
            }
        }
    } else {
        srv_mgr.safety_press_count = 0;
    }
#elif HAL_WITH_IO_MCU
    srv_mgr._safety_state = srv_hal_safety_switch_state();
    iomcu_set_safety_mask(srv_mgr.safety_mask);
#endif

#ifdef HAL_GPIO_PIN_LED_SAFETY
    srv_mgr.led_counter = (srv_mgr.led_counter+1) % 16;
    const uint16_t led_pattern = safety_state==SAFETY_DISARMED?0x5500:0xFFFF;
    //palWriteLine(HAL_GPIO_PIN_LED_SAFETY, (led_pattern & (1U << srv_mgr.led_counter))?0:1);
#endif
}

/*
  start corking output
 */
void srv_hal_cork(void)
{
    srv_mgr.corked = true;
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        iomcu_cork();
    }
#endif
}

/*
  stop corking output
 */
void srv_hal_push(void)
{
    srv_mgr.corked = false;
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        iomcu_push();
    }
#endif
}

void srv_hal_write(uint8_t chan, uint16_t period_us)
{
    if (chan >= max_channels) {
        return;
    }
    srv_mgr.last_sent[chan] = period_us;

#if HAL_WITH_IO_MCU
    // handle IO MCU channels
    if (brd_io_enabled()) {
        uint16_t io_period_us = period_us;
        if ((srv_mgr.iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & srv_mgr.io_fast_channel_mask)) {
            // the iomcu only has one oneshot setting, so we need to scale by a factor
            // of 8 here for oneshot125
            io_period_us /= 8;
        }
        iomcu_write_channel(chan, io_period_us);
    }
#endif
    if (chan < srv_mgr.chan_offset) {
        return;
    }

    if (srv_mgr._safety_state == SAFETY_DISARMED && !(srv_mgr.safety_mask & (1U<<chan))) {
        // implement safety pwm value
        period_us = 0;
    }

    chan -= srv_mgr.chan_offset;

    srv_mgr.period[chan] = period_us;

    if (chan < srv_mgr.num_fmu_channels) {
        srv_mgr.active_fmu_channels = MAX(chan+1, srv_mgr.active_fmu_channels);
        if (!srv_mgr.corked) {
            //push_local();
        }
    }
}

uint16_t srv_hal_read(uint8_t chan)
{
    if (chan >= max_channels) {
        return 0;
    }
#if HAL_WITH_IO_MCU
    if (chan < srv_mgr.chan_offset) {
        uint16_t period_us = iomcu_read_channel(chan);
        if ((srv_mgr.iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & srv_mgr.io_fast_channel_mask)) {
            // convert back to 1000 - 2000 range
            period_us *= 8;
        }
        return period_us;
    }
#endif
    chan -= srv_mgr.chan_offset;
    return srv_mgr.period[chan];
}

void srv_hal_read2(uint16_t* period_us, uint8_t len)
{
    if (len > max_channels) {
        len = max_channels;
    }
#if HAL_WITH_IO_MCU
    for (uint8_t i=0; i<MIN(len, srv_mgr.chan_offset); i++) {
        period_us[i] = iomcu_read_channel(i);
        if ((srv_mgr.iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<i) & srv_mgr.io_fast_channel_mask)) {
            // convert back to 1000 - 2000 range
            period_us[i] *= 8;
        }
    }
#endif
    if (len <= srv_mgr.chan_offset) {
        return;
    }
    len -= srv_mgr.chan_offset;
    period_us += srv_mgr.chan_offset;

    rt_memcpy(period_us, srv_mgr.period, len*sizeof(uint16_t));
}

/*
  enable sbus output
 */
bool srv_hal_enable_px4io_sbus_out(uint16_t rate_hz)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        return iomcu_enable_sbus_out(rate_hz);
    }
#endif
    return false;
}

/*
  set output frequency in HZ for a set of channels given by a mask
 */
void srv_hal_set_freq(uint32_t chmask, uint16_t freq_hz)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        // change frequency on IOMCU
        uint16_t io_chmask = chmask & 0xFF;
        if (io_chmask) {
            uint8_t count = iomcu_get_ch_masks_size();
            const uint8_t* ch_masks = iomcu_ch_masks();
            // disallow changing frequency of this group if it is greater than the default
            for (uint8_t i=0; i<count; i++) {
                const uint16_t mask = io_chmask & ch_masks[i];
                if (mask != 0) {
                    if (freq_hz > 50) {
                        srv_mgr.io_fast_channel_mask |= mask;
                    } else {
                        srv_mgr.io_fast_channel_mask &= ~mask;
                    }
                }
            }
            iomcu_set_freq(srv_mgr.io_fast_channel_mask, freq_hz);
        }
    }
#endif

    // convert to a local (FMU) channel mask
    chmask >>= srv_mgr.chan_offset;
    if (chmask == 0) {
        return;
    }

    /*
      we enable the new frequency on all groups that have one
      of the requested channels. This means we may enable high
      speed on some channels that aren't requested, but that
      is needed in order to fly a vehicle such as a hex
      multicopter properly
    */
#if 0
    for (auto &group : pwm_group_list) {
        // greater than 400 doesn't give enough room at higher periods for
        // the down pulse. This still allows for high rate with oneshot and dshot.
        uint16_t group_freq = freq_hz;
        if (group_freq > 400 && group.current_mode != MODE_PWM_BRUSHED) {
            group_freq = 400;
        }
        if ((group.ch_mask & chmask) != 0) {
            group.rc_frequency = group_freq;
            set_freq_group(group);
            // disallow changing frequency of this group if it is greater than the default
            if (group_freq > 50) {
                fast_channel_mask |= group.ch_mask;
            }
        }
    }
#endif
}

/*
  set default output rate
 */
void srv_hal_set_default_rate(uint16_t freq_hz)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        iomcu_set_default_rate(freq_hz);
    }
#endif
}

/* Output freq (1/period) control */
uint16_t srv_hal_get_freq(uint8_t chan)
{
#if HAL_WITH_IO_MCU
    if (chan < srv_mgr.chan_offset) {
        return iomcu_get_freq(chan);
    }
#endif

    // assume 50Hz default
    return 50;
}

void srv_hal_enable_ch(uint8_t chan)
{
    srv_mgr.en_mask |= 1U << (chan - srv_mgr.chan_offset);
}

void srv_hal_disable_ch(uint8_t chan)
{
    srv_mgr.en_mask &= ~(1U<<(chan - srv_mgr.chan_offset));
}

/*
  setup output mode
 */
void srv_hal_set_output_mode(uint16_t mask, const enum output_mode mode)
{
#if HAL_WITH_IO_MCU
    if ((mode == MODE_PWM_ONESHOT ||
         mode == MODE_PWM_ONESHOT125) &&
        (mask & ((1U<<srv_mgr.chan_offset)-1)) &&
        brd_io_enabled()) {
        srv_mgr.iomcu_mode = mode;
        // also setup IO to use a 1Hz frequency, so we only get output
        // when we trigger
        iomcu_set_freq(srv_mgr.io_fast_channel_mask, 1);
        iomcu_set_oneshot_mode();
        return;
    }
    if (mode == MODE_PWM_BRUSHED &&
        (mask & ((1U<<srv_mgr.chan_offset)-1)) &&
        brd_io_enabled()) {
        srv_mgr.iomcu_mode = mode;
        iomcu_set_brushed_mode();
        return;
    }
#endif
}

/*
  set PWM to send to a set of channels if the FMU firmware dies
*/
void srv_hal_set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
{
#if HAL_WITH_IO_MCU
    if (brd_io_enabled()) {
        iomcu_set_failsafe_pwm(chmask, period_us);
    }
#endif
}

void srv_hal_set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm)
{
    srv_mgr._esc_pwm_min = min_pwm;
    srv_mgr._esc_pwm_max = max_pwm;
}

/*------------------------------------test------------------------------------*/


